/*--------------------------------------------------------------------
 * File name: mb_parse_binary.c
 * Author: 刘长达      Version:   1.0     Date:
 * Description: 主要为读取函数具体的实现
 * Contents: 具体编写过程在头文件中
 *--------------------------------------------------------------------*/

#include "mb_parse_binary.h"
#define BYTESWAPPED

unsigned int mb_parse_binary::depth_t = 0;
unsigned int mb_parse_binary::xyz_t = 0;
unsigned int mb_parse_binary::seabed_t = 0;
unsigned int mb_parse_binary::range_t = 0;
unsigned int mb_parse_binary::heading_t = 0;
unsigned int mb_parse_binary::position_t = 0;
unsigned int mb_parse_binary::attitude_t = 0;
static char *line = NULL;
static char* readline(FILE *input);
static int max_line_len;

/*------------------函数定义区------------------------*/


void mb_parse_binary::func_get_binary(QString readfile)
{
    QFileInfo info(readfile);
    cout<<  "绝对路径"<<info.absoluteDir();
    cout<<"文件大小"<<info.size()/1024<<"KB";
    QFile file(readfile);
    bool openOK = file.open(QIODevice::ReadOnly);
    if(openOK == true) {
        QDataStream readbinary(&file);
        char read_len[6]; memset(read_len,0,6);

        unsigned int bytes, STX, type;
        bytes = STX = type = 0;
// 模仿创建        Coordinate *p = new Coordinate[3];
//         mbsys_depth_struct * depth_arr = new mbsys_depth_struct[depth_t];
        while(!readbinary.atEnd()) {
            readbinary.readRawData(read_len,6);
            mb_get_binary_int(&read_len[0],&bytes);
            memcpy(&STX,&read_len[4],1);
            memcpy(&type,&read_len[5],1);
            if(STX != 0x02) {
                cout<<"STX is error from rawdata";
                break;
            }
            switch (type) {
            case 0x44: {
                depth_t++;
                depth.depth_bytes = bytes;
                 depth.STX = STX; depth.Type= type;
                 char temp_char[depth.depth_bytes-2];
                 memset(temp_char,0,depth.depth_bytes-2);
                 readbinary.readRawData(temp_char,depth.depth_bytes-2); unsigned int index = 0;
                 mb_get_binary_short(&temp_char[index],&depth.EM_model); index += 2;
                 mb_get_binary_int(&temp_char[index],&depth.depth_date); index += 4;
                 mb_get_binary_int(&temp_char[index],&depth.depth_msec); index += 4 ;
                 mb_get_binary_short(&temp_char[index],&depth.depth_ping_count); index += 2;
                 mb_get_binary_short(&temp_char[index],&depth.depth_sys_serial); index += 2;
                 mb_get_binary_short(&temp_char[index],&depth.depth_heading); index += 2;
                 mb_get_binary_short(&temp_char[index],&depth.depth_ssv); index += 2;
                 mb_get_binary_short(&temp_char[index],&depth.depth_transmit_depth); index += 2;
                 depth.depth_beams_max = (mb_u_char)temp_char[index]; index++;
                 depth.depth_N = (mb_u_char)temp_char[index]; index++;
                 depth.depth_Zresoulation = (mb_u_char)temp_char[index]; index++;
                 depth.depth_XYresoulation = (mb_u_char)temp_char[index]; index++;
                 mb_get_binary_short(&temp_char[index],&depth.depth_HZsampling); index += 2;
                 for(unsigned int count = 0; count<depth.depth_N; count++) {
                     if (depth.EM_model == 120 || depth.EM_model == 300) {
                         mb_get_binary_short(&temp_char[index],&depth.depth_120and300_transmit_N[count]); index += 2; }
                     else {
                         mb_get_binary_short(&temp_char[index],&depth.depth_transmit_N[count]); index += 2; }
                     mb_get_binary_short(&temp_char[index],&depth.depth_acrosstrack_N[count]); index += 2;
                     mb_get_binary_short(&temp_char[index],&depth.depth_alongtrack_N[count]); index += 2;
                     mb_get_binary_short(&temp_char[index],&depth.depth_depression_N[count]); index += 2;
                     mb_get_binary_short(&temp_char[index],&depth.depth_azimuth_N[count]); index += 2;
                     mb_get_binary_short(&temp_char[index],&depth.depth_range_N[count]); index += 2;
                     depth.depth_quality_N[count] = (mb_u_char)temp_char[index]; index++;
                     depth.depth_detection_window[count] = (mb_u_char)temp_char[index]; index++;
                     depth.depth_reflectivity_N[count] = (mb_s_char)temp_char[index]; index++;
                     depth.depth_beam_num[count] = (mb_u_char)temp_char[index]; index++;
                 }
                 depth.depth_offset_multiplier = (mb_s_char)temp_char[index]; index++;
                 depth.ETX = (mb_u_char)temp_char[index]; index++;
                 mb_get_binary_short(&temp_char[index],&depth.check); index += 2;

                depth_cor.ping_count = depth.depth_ping_count;
                depth_cor.depth_msec = depth.depth_msec;
                depth_cor.beam_N = depth.depth_N;
                for(unsigned int count = 0; count<depth.depth_N; count++) {
                    // only for EM120 or EM300 now
                   depth_cor.depth_120and300_transmit_N[count] = depth.depth_120and300_transmit_N[count]/100;
                   depth_cor.depth_acrosstrack_N[count] = depth.depth_acrosstrack_N[count]/100;
                   depth_cor.depth_alongtrack_N[count] = depth.depth_alongtrack_N[count]/100;
//                   depth_cor.depth_depression_N[count] = depth.depth_depression_N[count];
//                   depth_cor.depth_azimuth_N[count] = depth.depth_azimuth_N[count];
//                   depth_cor.depth_range_N[count] = depth.depth_range_N[count];
//                   depth_cor.depth_quality_N[count] = depth.depth_quality_N[count];
//                   depth_cor.depth_detection_window[count] = depth.depth_detection_window[count];
//                   depth_cor.depth_reflectivity_N[count] = depth.depth_reflectivity_N[count];
//                   depth_cor.depth_beam_num[count] = depth.depth_beam_num[count];
                }
                depth_correct.push_back(depth_cor);
                 depth_arr.push_back(depth);
                 if(index != depth.depth_bytes-2) {
                     cout<<"Rawdata in depth["<<depth_t<<"] is wrong";
                     break;
                 }
            }
                break; // depth
            case 0x58: {
                xyz_t++;
                xyz.xyz_bytes = bytes;
                 xyz.STX = STX; xyz.Type= type;
                 char temp_char[xyz.xyz_bytes-2];
                 memset(temp_char,0,xyz.xyz_bytes-2);
                 readbinary.readRawData(temp_char,xyz.xyz_bytes-2); unsigned int index = 0;
                 mb_get_binary_short(&temp_char[index],&xyz.EM_model); index += 2;
                 mb_get_binary_int(&temp_char[index],&xyz.xyz_date); index += 4;
                 mb_get_binary_int(&temp_char[index],&xyz.xyz_msec); index += 4 ;
                 mb_get_binary_short(&temp_char[index],&xyz.xyz_ping_count); index += 2;
                 mb_get_binary_short(&temp_char[index],&xyz.xyz_sys_serial); index += 2;
                 mb_get_binary_short(&temp_char[index],&xyz.xyz_heading); index += 2;
                 mb_get_binary_short(&temp_char[index],&xyz.xyz_ssv); index += 2;
                 mb_get_binary_float(&temp_char[index],&xyz.xyz_transmit_depth); index += 4;
                 mb_get_binary_short(&temp_char[index],&xyz.xyz_N); index += 2;
                 mb_get_binary_short(&temp_char[index],&xyz.xyz_valid_detection); index += 2;
                 mb_get_binary_float(&temp_char[index],&xyz.xyz_HZsampling); index += 4;
                 xyz.xyz_Scanning = (mb_u_char)temp_char[index]; index++;
                 index += 3;
                 for(unsigned int count = 0; count<xyz.xyz_N; count++) {
                     mb_get_binary_float(&temp_char[index],&xyz.xyz_depth_N); index += 4;
                     mb_get_binary_float(&temp_char[index],&xyz.xyz_acrosstrack_N); index += 4;
                     mb_get_binary_float(&temp_char[index],&xyz.xyz_alongtrack_N); index += 4;
                     mb_get_binary_short(&temp_char[index],&xyz.xyz_detection_window_N[count]); index += 2;
                     xyz.xyz_quality_N[count] = (mb_u_char)temp_char[index]; index++;
                     xyz.xyz_IBA_N[count] = (mb_s_char)temp_char[index]; index++;
                     xyz.xyz_detection_info[count] = (mb_u_char)temp_char[index]; index++;
                     xyz.xyz_time_cleaning[count] = (mb_s_char)temp_char[index]; index++;
                     mb_get_binary_short(&temp_char[index],&xyz.xyz_BS_resloution[count]); index += 2;
                 }
                 xyz.Spare = (mb_u_char)temp_char[index]; index++;
                 xyz.ETX = (mb_u_char)temp_char[index]; index++;
                 mb_get_binary_short(&temp_char[index],&xyz.check); index += 2;
/*  此为进行修正的数据结构体 暂时未做处理
                depth_cor.ping_count = depth.depth_ping_count;
                depth_cor.depth_msec = depth.depth_msec;
                depth_cor.beam_N = depth.depth_N;
                for(unsigned int count = 0; count<depth.depth_N; count++) {
                    // only for EM120 or EM300 now
                   depth_cor.depth_120and300_transmit_N[count] = depth.depth_120and300_transmit_N[count];
                   depth_cor.depth_acrosstrack_N[count] = depth.depth_acrosstrack_N[count];
                   depth_cor.depth_alongtrack_N[count] = depth.depth_alongtrack_N[count];
                   depth_cor.depth_depression_N[count] = depth.depth_depression_N[count];
                   depth_cor.depth_azimuth_N[count] = depth.depth_azimuth_N[count];
                   depth_cor.depth_range_N[count] = depth.depth_range_N[count];
                   depth_cor.depth_quality_N[count] = depth.depth_quality_N[count];
                   depth_cor.depth_detection_window[count] = depth.depth_detection_window[count];
                   depth_cor.depth_reflectivity_N[count] = depth.depth_reflectivity_N[count];
                   depth_cor.depth_beam_num[count] = depth.depth_beam_num[count];
                }
                depth_correct.push_back(depth_cor);
                 depth_arr.push_back(depth);*/
                 xyz_arr.push_back(xyz);
//                 if(index != depth.depth_bytes-2) {
//                     cout<<"Rawdata in depth["<<depth_t<<"] is wrong";
//                     break;
//                 }
            }
                break; // xyz
            case 0x53: {
                seabed_t++;
                seabed.seabed_bytes = bytes;
                 seabed.STX = STX; seabed.Type= type;
                 char temp_char[bytes-2]; memset(temp_char,0,bytes-2);
                 readbinary.readRawData(temp_char,bytes-2); int index = 0;
                 mb_get_binary_short(&temp_char[index],&seabed.EM_model); index += 2;
                 mb_get_binary_int(&temp_char[index],&seabed.seabed_date); index += 4;
                 mb_get_binary_int(&temp_char[index],&seabed.seabed_msec); index += 4 ;
                 mb_get_binary_short(&temp_char[index],&seabed.seabed_count); index += 2;
                 mb_get_binary_short(&temp_char[index],&seabed.seabed_serial); index += 2;
                 mb_get_binary_short(&temp_char[index],&seabed.seabed_absorption); index += 2;
                 mb_get_binary_short(&temp_char[index],&seabed.seabed_pluse_len); index += 2;
                 mb_get_binary_short(&temp_char[index],&seabed.seabed_range_samples); index += 2;
                 mb_get_binary_short(&temp_char[index],&seabed.seabed_range_TVG_start); index += 2;
                 mb_get_binary_short(&temp_char[index],&seabed.seabed_range_TVG_stop); index += 2;
                 seabed.seabed_BSN = (mb_s_char)temp_char[index]; index++;
                 seabed.seabed_BSO = (mb_s_char)temp_char[index]; index++;
                 mb_get_binary_short(&temp_char[index],&seabed.seabed_beamwidth_TX); index += 2;
                 seabed.seabed_crossover_TVG = (mb_u_char)temp_char[index]; index++;
                 seabed.seabed_N = (mb_u_char)temp_char[index]; index++;
                 seabed.count_NS = 0;
                 for(unsigned int count = 0; count<seabed.seabed_N; count++) {
                     seabed.seabed_beam_index_N[count] = (mb_s_char)temp_char[index]; index++;
                     seabed.seabed_direction_N[count] = (mb_s_char)temp_char[index]; index++;
                     mb_get_binary_short(&temp_char[index],&seabed.seabed_Ns_N[count]); index += 2;
                     mb_get_binary_short(&temp_char[index],&seabed.seabed_centre_num_N[count]); index += 2;
                     seabed.count_NS = seabed.count_NS + seabed.seabed_Ns_N[count];
                 }
                 for(unsigned int count = 0; count<seabed.count_NS; count++) {
                 // memcpy(&seabed.seabed_ampulitudes_NS[count],&temp_char[index],1); index++;
                     seabed.seabed_ampulitudes_NS[count] = (mb_s_char)temp_char[index]; index++;
                 }
                 seabed.ETX = (mb_u_char)temp_char[index]; index++;
                 seabed_arr.push_back(seabed);
                 if(0x03 != seabed.ETX) {
                     cout<<"Rawdata in seabed["<<seabed_t<<"] is no spare";
                     break; }
            }
                break; // seabed image
           /* case 0x4e: {
                range_t++;
                range78.range78_bytes = bytes;
                 range78.STX = STX; range78.Type= type;
                 char temp_char[bytes-2]; memset(temp_char,0,bytes-2);
                 readbinary.readRawData(temp_char,bytes-2); int index = 0;
                 mb_get_binary_short(&temp_char[index],&range78.EM_model); index += 2;
                 mb_get_binary_int(&temp_char[index],&range78.); index += 4;
                 mb_get_binary_int(&temp_char[index],&range.seabed_msec); index += 4 ;
                 mb_get_binary_short(&temp_char[index],&range.seabed_count); index += 2;
                 mb_get_binary_short(&temp_char[index],&range.seabed_serial); index += 2;
                 mb_get_binary_short(&temp_char[index],&range.seabed_absorption); index += 2;
                 mb_get_binary_short(&temp_char[index],&range.seabed_pluse_len); index += 2;
                 mb_get_binary_short(&temp_char[index],&range.seabed_range_samples); index += 2;
                 mb_get_binary_short(&temp_char[index],&range.seabed_range_TVG_start); index += 2;
                 mb_get_binary_short(&temp_char[index],&range.seabed_range_TVG_stop); index += 2;
                 range.seabed_BSN = (mb_s_char)temp_char[index]; index++;
                 range.seabed_BSO = (mb_s_char)temp_char[index]; index++;
                 mb_get_binary_short(&temp_char[index],&range.seabed_beamwidth_TX); index += 2;
                 range.seabed_crossover_TVG = (mb_u_char)temp_char[index]; index++;
                 range.seabed_N = (mb_u_char)temp_char[index]; index++;
                 range.count_NS = 0;
                 for(unsigned int count = 0; count<range.seabed_N; count++) {
                     range.seabed_beam_index_N[count] = (mb_s_char)temp_char[index]; index++;
                     range.seabed_direction_N[count] = (mb_s_char)temp_char[index]; index++;
                     mb_get_binary_short(&temp_char[index],&range.seabed_Ns_N[count]); index += 2;
                     mb_get_binary_short(&temp_char[index],&range.seabed_centre_num_N[count]); index += 2;
                     range.count_NS = range.count_NS + range.seabed_Ns_N[count];
                 }
                 for(unsigned int count = 0; count<range.count_NS; count++) {
                 // memcpy(&range.seabed_ampulitudes_NS[count],&temp_char[index],1); index++;
                     range.seabed_ampulitudes_NS[count] = (mb_s_char)temp_char[index]; index++;
                 }
                 range.ETX = (mb_u_char)temp_char[index]; index++;
                 seabed_arr.push_back(seabed);
            }
                break; // range 78    */
            case 0x66: {
                range_t++;
                rangef.rangef_bytes = bytes;
                rangef.STX = STX; rangef.Type= type;
                char temp_char[bytes-2]; memset(temp_char,0,bytes-2);
                readbinary.readRawData(temp_char,bytes-2); int index = 0;
                mb_get_binary_short(&temp_char[index],&rangef.EM_model); index += 2;
                mb_get_binary_int(&temp_char[index],&rangef.rangef_date); index += 4;
                mb_get_binary_int(&temp_char[index],&rangef.rangef_msec); index += 4 ;
                mb_get_binary_short(&temp_char[index],&rangef.rangef_ping_count); index += 2;
                mb_get_binary_short(&temp_char[index],&rangef.rangef_serial); index += 2;
                mb_get_binary_short(&temp_char[index],&rangef.rangef_Ntx); index += 2;
                mb_get_binary_short(&temp_char[index],&rangef.rangef_N); index += 2;
                mb_get_binary_int(&temp_char[index],&rangef.rangef_SamplingHZ); index += 4;
                mb_get_binary_int(&temp_char[index],&rangef.rangef_ROV); index += 4;
                mb_get_binary_short(&temp_char[index],&rangef.rangef_ssv); index += 2;
                mb_get_binary_short(&temp_char[index],&rangef.rangef_maxbeam_possi); index += 2;
                mb_get_binary_short(&temp_char[index],&rangef.Spare1); index += 2;
                mb_get_binary_short(&temp_char[index],&rangef.Spare2); index += 2;
                for(unsigned int count = 0; count<rangef.rangef_Ntx; count++) {
                    mb_get_binary_short(&temp_char[index],&rangef.rangef_angleTX_Ntx[count]); index += 2;
                    mb_get_binary_short(&temp_char[index],&rangef.rangef_focus_Ntx[count]); index += 2;
                    mb_get_binary_int(&temp_char[index],&rangef.rangef_signal_len_Ntx); index += 4;
                    mb_get_binary_int(&temp_char[index],&rangef.rangef_timeoff_Ntx); index += 4;
                    mb_get_binary_int(&temp_char[index],&rangef.rangef_centre_hz_Ntx); index += 4;
                    mb_get_binary_short(&temp_char[index],&rangef.rangef_bandwidth_hz_Ntx); index += 2;
                    rangef.rangef_signal_waveform_Ntx[count] = (mb_u_char)temp_char[index]; index++;
                    rangef.rangef_transmit_sector_Ntx[count] = (mb_u_char)temp_char[index]; index++;
                }
                for(unsigned int count = 0; count<rangef.rangef_N; count++) {
                    mb_get_binary_short(&temp_char[index],&rangef.rangef_anlge_RX_N[count]); index += 2;
                    mb_get_binary_short(&temp_char[index],&rangef.rangef_R_N[count]); index += 2;
                    rangef.rangef_transmit_sector_num_N[count] = (mb_u_char)temp_char[index]; index++;
                    rangef.rangef_Reflectivity_N[count] = (mb_s_char)temp_char[index]; index++;
                    rangef.rangef_quality_N[count] = (mb_u_char)temp_char[index]; index++;
                    rangef.rangef_detection_len_N[count] = (mb_u_char)temp_char[index]; index++;
                    mb_get_binary_short(&temp_char[index],&rangef.rangef_beam_number_N[count]); index += 2;
                    mb_get_binary_short(&temp_char[index],&rangef.Spare); index += 2;
                }
                rangef.Spare = (mb_u_char)temp_char[index]; index++; // alwaays==00
                rangef.ETX = (mb_u_char)temp_char[index]; index++; // alwaays==03h
                rangef_arr.push_back(rangef);
            }
                break; // range f
            case 0x41: {
                attitude_t++;
                attitude.attitude_bytes = bytes;
                 attitude.STX = STX; attitude.Type= type;
                 char temp_char[bytes-2]; memset(temp_char,0,bytes-2);
                 readbinary.readRawData(temp_char,bytes-2); unsigned int index = 0;
                 mb_get_binary_short(&temp_char[index],&attitude.EM_model); index += 2;
                 mb_get_binary_int(&temp_char[index],&attitude.attitude_date); index += 4;
                 mb_get_binary_int(&temp_char[index],&attitude.attitude_msec); index += 4 ;
                 mb_get_binary_short(&temp_char[index],&attitude.attitude_count); index += 2;
                 mb_get_binary_short(&temp_char[index],&attitude.attitude_serial); index += 2;
                 mb_get_binary_short(&temp_char[index],&attitude.attitude_N); index += 2;
                 for(unsigned int count = 0; count < attitude.attitude_N; count++) {
                     mb_get_binary_short(&temp_char[index],&attitude.attitude_msectime_N[count]); index += 2;
                     mb_get_binary_short(&temp_char[index],&attitude.attitude_status_N[count]); index += 2;
                     mb_get_binary_short(&temp_char[index],&attitude.attitude_roll_N[count]); index += 2;
                     mb_get_binary_short(&temp_char[index],&attitude.attitude_pitch_N[count]); index += 2;
                     mb_get_binary_short(&temp_char[index],&attitude.attitude_heave_N[count]); index += 2;
                     mb_get_binary_short(&temp_char[index],&attitude.attitude_heading_N[count]); index += 2;
                 }
                 attitude.attitude_sensor_descriptor = (mb_u_char)temp_char[index]; index++;
                 attitude.ETX = (mb_u_char)temp_char[index]; index++;
                 mb_get_binary_short(&temp_char[index],&attitude.cheack); index += 2;
                 attitude_cor.attitude_date = attitude.attitude_date;
                 attitude_cor.attitude_mesc = attitude.attitude_msec;
                 attitude_cor.attitude_count = attitude.attitude_count;
                for(unsigned int count_N = 0; count_N < attitude.attitude_N; count_N++) {
                    attitude_cor.attitude_msectime_N[count_N] = attitude.attitude_msec + attitude.attitude_msectime_N[count_N];
                    attitude_cor.attitude_roll_N[count_N] = (double)attitude.attitude_roll_N[count_N] / 100;
                    attitude_cor.attitude_pitch_N[count_N] = (double)attitude.attitude_pitch_N[count_N] / 100;
                    attitude_cor.attitude_heave_N[count_N] = (double)attitude.attitude_heave_N[count_N] /100;
                    attitude_cor.attitude_heading_N[count_N] = (double)attitude.attitude_heading_N[count_N] / 100;
                }
                 attitude_correct.push_back(attitude_cor);
                 attitude_arr.push_back(attitude);
                 if(index != attitude.attitude_bytes-2) {
                     cout<<"Rawdata in attitude["<<attitude_t<<"] is wrong";
                     break;
                 }


            }
                break; // attitude
            case 0x50: {
                position_t++;
                position.position_bytes = bytes;
                 position.STX = STX; position.Type= type;
                 char temp_char[bytes-2]; memset(temp_char,0,bytes-2);
                 readbinary.readRawData(temp_char,bytes-2); int index = 0;
                 mb_get_binary_short(&temp_char[index],&position.EM_model); index += 2;
                 mb_get_binary_int(&temp_char[index],&position.position_date); index += 4;
                 mb_get_binary_int(&temp_char[index],&position.position_msec); index += 4 ;
                 mb_get_binary_short(&temp_char[index],&position.position_count); index += 2;
                 mb_get_binary_short(&temp_char[index],&position.position_serial); index += 2;
                 mb_get_binary_int(&temp_char[index],&position.position_latitude); index += 4;
                 mb_get_binary_int(&temp_char[index],&position.position_longitude); index += 4 ;
                 mb_get_binary_short(&temp_char[index],&position.position_measure); index += 2;
                 mb_get_binary_short(&temp_char[index],&position.position_vessel_speed); index += 2;
                 mb_get_binary_short(&temp_char[index],&position.position_vessel_course); index += 2;
                 mb_get_binary_short(&temp_char[index],&position.position_vessel_heading); index += 2;
                 position.position_descriptor = (mb_u_char)temp_char[index]; index++;
                 position.position_inputN = (mb_u_char)temp_char[index]; index++; //之后暂不解析

                 position_cor.position_B = (double)position.position_latitude / 10000000;
                 position_cor.position_L = (double)position.position_longitude / 20000000 ;
                 position_cor.position_msec = position.position_msec;
                 position_cor.position_date = position.position_date;
                 position_correct.push_back(position_cor);
                 position_arr.push_back(position);                  
            }
                break; // position
            case 0x43: {
                heading_t++;
                heading.heading_bytes = bytes;
                 heading.STX = STX; heading.Type= type;
                 char temp_char[bytes-2]; memset(temp_char,0,bytes-2);
                 readbinary.readRawData(temp_char,bytes-2); int index = 0;
                 mb_get_binary_short(&temp_char[index],&heading.EM_model); index += 2;
                 mb_get_binary_int(&temp_char[index],&heading.heading_date); index += 4;
                 mb_get_binary_int(&temp_char[index],&heading.heading_msec); index += 4 ;
                 mb_get_binary_short(&temp_char[index],&heading.heading_count); index += 2;
                 mb_get_binary_short(&temp_char[index],&heading.heading_serial); index += 2;
                 mb_get_binary_short(&temp_char[index],&heading.heading_N); index += 2;
                 for(unsigned int i = 0; i<heading.heading_N; i++) {
                     mb_get_binary_short(&temp_char[index],&heading.heading_msecN[i]); index += 2;
                     mb_get_binary_short(&temp_char[index],&heading.headingN[i]); index += 2 ;
                 }
                 heading.ETX = (mb_u_char)temp_char[index]; index++;
                 if(heading.ETX != 00)
                     cout<<"Rawdata in heading["<<heading_t<<"] is no spare";
            }
                break; // heading
/*            case 0x52: {
//                runtime_t++;
//                runtime.runtime_bytes = bytes;
//                 runtime.STX = STX; runtime.Type= type;
//                 char temp_char[bytes-2]; memset(temp_char,0,bytes-2);
//                 readbinary.readRawData(temp_char,bytes-2); int index = 0;
//                 mb_get_binary_short(&temp_char[index],&runtime.EM_model); index += 2;
//                 mb_get_binary_int(&temp_char[index],&runtime.runtime_date); index += 4;
//                 mb_get_binary_int(&temp_char[index],&runtime.runtime_msec); index += 4 ;
//                 mb_get_binary_short(&temp_char[index],&runtime.runtime_count); index += 2;
//                 mb_get_binary_short(&temp_char[index],&runtime.runtime_serial); index += 2;
//                 runtime.runtime_status = (mb_u_char)temp_char[index]; index++;
//                 runtime.runtime_CPU = (mb_u_char)temp_char[index]; index++;
//                 runtime.runtime_BSP = (mb_u_char)temp_char[index]; index++;
//                 runtime.runtime_header_status = (mb_u_char)temp_char[index]; index++;
//                 runtime.runtime_mode = (mb_u_char)temp_char[index]; index++;
//                 runtime.runtime_filter = (mb_u_char)temp_char[index]; index++;
//                 mb_get_binary_short(&temp_char[index],&runtime.runtime_min); index += 2;
//                 mb_get_binary_short(&temp_char[index],&runtime.runtime_max); index += 2;
//                 mb_get_binary_short(&temp_char[index],&runtime.runtime_absorption); index += 2;
//                 mb_get_binary_short(&temp_char[index],&runtime.runtime_transmit_len); index += 2;
//                 mb_get_binary_short(&temp_char[index],&runtime.runtime_transmit_beamwidth); index += 2;
//                 runtime.runtime_transmit_power= (mb_s_char)temp_char[index]; index++;
//                 runtime.runtime_rece_beamwid = (mb_u_char)temp_char[index]; index++;
//                 runtime.runtime_beamwidth_hz = (mb_u_char)temp_char[index]; index++;
//                 runtime.runtime_mode2 = (mb_u_char)temp_char[index]; index++;
//                 runtime.runtime_TVG = (mb_u_char)temp_char[index]; index++;
//                 runtime.runtime_sur_sped = (mb_u_char)temp_char[index]; index++;
//                 mb_get_binary_short(&temp_char[index],&runtime.runtime_max_swath); index += 2;
//                 runtime.runtime_space_beam = (mb_u_char)temp_char[index]; index++;
//                 runtime.runtime_max_cover = (mb_u_char)temp_char[index]; index++;
//                 runtime.runtime_yaw = (mb_u_char)temp_char[index]; index++;
//                 runtime.runtime_max_starboard = (mb_u_char)temp_char[index]; index++;
//                 mb_get_binary_short(&temp_char[index],&runtime.runtime_max_starboard_m); index += 2;
//                 mb_get_binary_short(&temp_char[index],&runtime.runtime_durotong_sped); index += 2;
//                 runtime.runtime_fileter_or_hilo = (mb_u_char)temp_char[index]; index++;
//                 runtime.ETX = (mb_u_char)temp_char[index]; index++;
//                 mb_get_binary_short(&temp_char[index],&runtime.check); index += 2;
//            }
//                break; */ // runtime
            default: {
                char temp_char[bytes-2];
                readbinary.readRawData(temp_char,2);
                readbinary.readRawData(temp_char,2);
                readbinary.readRawData(temp_char,bytes-6);
//                bool seekOK= QIODevice::seek(bytes-2);
//                if(seekOK == false) {
//                   cout<<"The rawdata seek is wrong";
//               }
            }
                break;
            }
        }
    }
    file.close();
}
/* Name: func_print_binary
 * Function: 打印原始的数据
 * Inpute Paraments:
 * Outpute Paraments:
 * Return:
 * How:
 * Debug:
 * Questions:
 *
 */
void mb_parse_binary::func_print_binary(QString writefile)
{
    QFile file(writefile);
    bool openOK = file.open(QIODevice::WriteOnly);
    if(openOK == true) {
        QTextStream write(&file);
        for(unsigned int j=0; j<depth_t; j++) {
        write<<depth_arr[j].depth_bytes<<" "<<depth_arr[j].STX<<" "<<depth_arr[j].Type<<" "<<depth_arr[j].depth_date<<" \n"
            <<depth_arr[j].depth_msec<<" "<<depth_arr[j].depth_ping_count<<" "<<depth_arr[j].depth_sys_serial<<" "<<depth_arr[j].depth_heading<<endl
           <<depth_arr[j].depth_ssv<<" "<<depth_arr[j].depth_transmit_depth<<" "<<depth_arr[j].depth_beams_max<<" "<<depth_arr[j].depth_N<<endl
          <<depth_arr[j].depth_Zresoulation<<" "<<depth_arr[j].depth_XYresoulation<<" "<<depth_arr[j].depth_HZsampling<<endl;
        for(unsigned int count = 0; count<depth_arr[j].depth_N; count++) {
            write<<depth_arr[j].depth_120and300_transmit_N[count]<<" "<<depth_arr[j].depth_acrosstrack_N[count]<<" "<<depth_arr[j].depth_alongtrack_N[count]
                   <<" "<<depth_arr[j].depth_depression_N[count]<<" "<<depth_arr[j].depth_azimuth_N[count]<<endl
                  <<depth_arr[j].depth_range_N[count]<<" "<<depth_arr[j].depth_quality_N[count]<<" "<<depth_arr[j].depth_detection_window[count]
                    <<" "<<depth_arr[j].depth_reflectivity_N[count]<<" "<<depth_arr[j].depth_beam_num[count]<<endl;
        }
        write<<depth_arr[j].depth_offset_multiplier<<" "<<depth_arr[j].ETX<<" "<<depth_arr[j].check<<" ETX"<<endl;
        }
        write<<"seabed "<<endl;
        for(unsigned int j = 0; j < seabed_t; j++) {
            write<<seabed_arr[j].seabed_bytes<<" ";
            write<<seabed_arr[j].STX<<" ";
            write<<seabed_arr[j].Type<<" ";
            write<<seabed_arr[j].EM_model<<endl;
            write<<seabed_arr[j].seabed_date<<" ";
            write<<seabed_arr[j].seabed_msec<<" ";
            write<<seabed_arr[j].seabed_count<<" ";
            write<<seabed_arr[j].seabed_serial<<endl;
            write<<seabed_arr[j].seabed_absorption<<" ";
            write<<seabed_arr[j].seabed_pluse_len<<" ";
            write<<seabed_arr[j].seabed_range_samples<<" ";
            write<<seabed_arr[j].seabed_range_TVG_start<<endl;
            write<<seabed_arr[j].seabed_range_TVG_stop<<" ";
            write<<seabed_arr[j].seabed_BSN<<" ";
            write<<seabed_arr[j].seabed_BSO<<" ";
            write<<seabed_arr[j].seabed_beamwidth_TX<<endl;
            write<<seabed_arr[j].seabed_crossover_TVG<<" ";
            write<<seabed_arr[j].seabed_N<<endl;
            for(unsigned int count = 0; count < seabed_arr[j].seabed_N; count++) {
                write<<seabed_arr[j].seabed_beam_index_N[count]<<" ";
                write<<seabed_arr[j].seabed_direction_N[count]<<" ";
                write<<seabed_arr[j].seabed_Ns_N[count]<<" ";
                write<<seabed_arr[j].seabed_centre_num_N[count]<<endl;
            }
            for(unsigned int count = 0; count < seabed_arr[j].count_NS; count++) {
                write<<seabed_arr[j].seabed_ampulitudes_NS[count]<<" ";
                if(0 == count%4)
                    write<<endl;
            }
            write<<seabed_arr[j].ETX<<endl;
            write<<seabed_arr[j].check<<endl;
        }
        write<<"position "<<endl;
        for(unsigned int j = 0; j < position_t; j++) {
            write<<position_arr[j].position_bytes<<" ";
            write<<position_arr[j].Type<<" ";
            write<<position_arr[j].EM_model<<" ";
            write<<position_arr[j].position_date<<" ";
            write<<position_arr[j].position_msec<<endl;
            write<<position_arr[j].position_count<<" ";
            write<<position_arr[j].position_serial<<" ";
            write<<position_arr[j].position_latitude<<" ";
            write<<position_arr[j].position_longitude<<" ";
            write<<position_arr[j].position_measure<<endl;
            write<<position_arr[j].position_vessel_speed<<" ";
            write<<position_arr[j].position_vessel_course<<" ";
            write<<position_arr[j].position_vessel_heading<<" ";
            write<<position_arr[j].position_descriptor<<" ";
            write<<position_arr[j].position_inputN<<endl;
        }
        write<<"attitude "<<endl;
        for(unsigned int j = 0; j < attitude_t; j++) {
            write<<attitude_arr[j].attitude_bytes<<" ";
            write<<attitude_arr[j].STX<<" ";
            write<<attitude_arr[j].Type<<" ";
            write<<attitude_arr[j].EM_model<<" ";
            write<<attitude_arr[j].attitude_date<<" ";
            write<<attitude_arr[j].attitude_msec<<" ";
            write<<attitude_arr[j].attitude_count<<" ";
            write<<attitude_arr[j].attitude_serial<<" ";
            write<<attitude_arr[j].attitude_N<<endl;
            for(unsigned int count = 0; count < attitude.attitude_N; count++) {
                write<<attitude_arr[j].attitude_msectime_N[count]<<" ";
                write<<attitude_arr[j].attitude_status_N[count]<<" ";
                write<<attitude_arr[j].attitude_roll_N[count]<<" ";
                write<<attitude_arr[j].attitude_pitch_N[count]<<" ";
                write<<attitude_arr[j].attitude_heave_N[count]<<" ";
                write<<attitude_arr[j].attitude_heading_N[count]<<endl;
            }
        }
    }
    /* 分别输出的输出格式
  *    if(openOK == true) {
            QTextStream write(&file);
            write<<"xyz "<<endl;
            for(unsigned int j=0; j<xyz_t; j++) {
                write<<xyz_arr[j].xyz_date<<"  "<<xyz_arr[j].xyz_msec<<" "<<xyz_arr[j].xyz_ping_count<<" "<<xyz_arr[j].xyz_N<<endl;
                for(unsigned int count = 0; count<xyz_arr[j].xyz_N; count++) {
                    write<<count<<" "<<xyz_arr[j].xyz_depth_N<<" "
                       <<xyz_arr[j].xyz_acrosstrack_N[count]<<" "<<xyz_arr[j].xyz_alongtrack_N[count]<<endl;
                }
            }
            write<<"position "<<endl;
            for(unsigned int j = 0; j < position_t; j++) {
                write<<position_arr[j].position_date<<" ";
                write<<position_arr[j].position_msec<<" ";
                write<<position_arr[j].position_count<<" ";
                write<<position_arr[j].position_latitude<<" ";
                write<<position_arr[j].position_longitude<<endl;
            }
            write<<"attitude "<<endl;
            for(unsigned int j = 0; j < attitude_t; j++) {
                write<<attitude_arr[j].attitude_date<<" ";
                write<<attitude_arr[j].attitude_msec<<" ";
                write<<attitude_arr[j].attitude_count<<" ";
                for(unsigned int count = 0; count < attitude.attitude_N; count++) {
                    write<<count<<" ";
                    write<<attitude_arr[j].attitude_msectime_N[count]<<" ";
                    write<<attitude_arr[j].attitude_roll_N[count]<<" ";
                    write<<attitude_arr[j].attitude_pitch_N[count]<<" ";
                    write<<attitude_arr[j].attitude_heave_N[count]<<" ";
                    write<<attitude_arr[j].attitude_heading_N[count]<<endl;
                }
            }
    }*/
    file.close();
}
void mb_parse_binary::func_print_depth(QString writefile) {
    QFile file(writefile);
    bool openOK = file.open(QIODevice::WriteOnly);
    if(openOK == true) {
        QTextStream write(&file);
        for(unsigned int j=0; j<depth_t; j++) {
            write<<depth_arr[j].depth_ping_count<<" ";
            write<<depth_arr[j].depth_N<<" ";
            write<<depth_arr[j].depth_date<<" ";
            write<<depth_arr[j].depth_msec<<endl;
            for(unsigned int count = 0; count<depth_arr[j].depth_N; count++) {
                write<<count+1<<" ";
                write<<depth_arr[j].depth_120and300_transmit_N[count]<<" ";
                write<<depth_arr[j].depth_acrosstrack_N[count]<<" ";
                write<<depth_arr[j].depth_alongtrack_N[count]<<" ";
                write<<rangef_arr[j].rangef_anlge_RX_N[count]<<" ";
                write<<rangef_arr[j].rangef_Reflectivity_N[count]<<endl;
            }
        }
    }
    file.close();
}
void mb_parse_binary::func_print_postion(QString writefile) {
    QFile file(writefile);
    bool openOK = file.open(QIODevice::WriteOnly);
    if(openOK == true) {
        QTextStream write(&file);
        for(unsigned int j = 0; j < position_t; j++) {
             write<<position_arr[j].position_date<<" ";
             write<<position_arr[j].position_msec<<" ";
             write<<position_arr[j].position_latitude<<" ";
             write<<position_arr[j].position_longitude<<endl;
         }
    }
    file.close();
}
void mb_parse_binary::func_print_seabed(QString writefile) {
    QFile file(writefile);
    bool openOK = file.open(QIODevice::WriteOnly);
    if(openOK == true) {
        QTextStream write(&file);
        write<<"seabed "<<endl;
        for(unsigned int j = 0; j < seabed_t; j++) {
            for(unsigned int count = 0; count < seabed_arr[j].seabed_N; count++) {
               write<<seabed_arr[j].seabed_count<<" ";
               write<<seabed_arr[j].seabed_N<<" ";
                write<<count + 1<<" ";
                write<<seabed_arr[j].seabed_date<<" ";
                write<<seabed_arr[j].seabed_msec<<" ";
                write<<seabed_arr[j].seabed_Ns_N[count]<<" ";
                for(unsigned int count_ns = 0; count_ns < seabed_arr[j].seabed_Ns_N[count]; count_ns++) {
                    write<<seabed_arr[j].seabed_ampulitudes_NS[count_ns]<<" ";
                }
                write<<endl;
            }

        }
    }
    file.close();
}
void mb_parse_binary::func_print_attitude(QString writefile) {
    QFile file(writefile);
    bool openOK = file.open(QIODevice::WriteOnly);
    if(openOK == true) {
        QTextStream write(&file);
        for(unsigned int j = 0; j < attitude_t; j++) {
             for(unsigned int count = 0; count < attitude.attitude_N; count++) {
                 write<<attitude_arr[j].attitude_date<<" ";
                 write<<attitude_arr[j].attitude_msec + attitude_arr[j].attitude_msectime_N[count]<<" ";
                 write<<attitude_arr[j].attitude_roll_N[count]<<" ";
                 write<<attitude_arr[j].attitude_pitch_N[count]<<" ";
                 write<<attitude_arr[j].attitude_heave_N[count]<<" ";
                 write<<attitude_arr[j].attitude_heading_N[count]<<endl;
             }
         }
    }
    file.close();
}

/* Function Name:Data_Read
 * Inpute Paraments:
 * Outpute Paraments:
 * Return:
 * How:
 * Debug:
 * Questions:
 */
void mb_parse_binary::Data_Read(QString readfile) {
    QFile file(readfile);
    bool openOK = file.open(QIODevice::ReadOnly | QIODevice::Text);
    if(openOK == true) {
        QTextStream read(&file);
        QString line = read.readLine(); QStringList list1;
        while (!read.atEnd()) {
            line = read.readLine(); list1=line.split(" ");
            depth_cor.ping_count=list1[0].toUInt();
            depth_cor.beam_N=list1[1].toUInt();
            depth_cor.depth_date=list1[2].toUInt();
            depth_cor.depth_msec=list1[3].toUInt();
            for(unsigned int i = 0; i < depth_cor.beam_N; i++) {
                line = read.readLine(); list1=line.split(" ");
                depth_cor.depth_120and300_transmit_N[i]=list1[1].toUInt();
                depth_cor.depth_acrosstrack_N[i]=list1[2].toShort();
                depth_cor.depth_alongtrack_N[i]=list1[3].toShort();
                depth_cor.Anlge_N[i]=list1[4].toShort();
                depth_cor.Reflectivity_N[i]=list1[5].toShort();
            }
            depth_correct.push_back(depth_cor);
        }
    }
    else {
        cout << "Open failed." << endl;
    }
    file.close(); 
}
void mb_parse_binary::Data_Read_Attitude(QString readfile) {
    QFile file(readfile);
    bool openOK = file.open(QIODevice::ReadOnly | QIODevice::Text);
    if(openOK == true) {
        QTextStream read(&file);
        QString line = read.readLine(); QStringList list2;
        while (!read.atEnd()) {
            line = read.readLine(); list2=line.split(" ");
            attitude_cor.attitude_date=list2[0].toUInt();
            attitude_cor.attitude_msectime_N[0]=list2[1].toUInt();
            attitude_cor.attitude_mesc = attitude_cor.attitude_msectime_N[0];
            attitude_cor.attitude_roll_N[0]=list2[2].toDouble()/100;
            attitude_cor.attitude_pitch_N[0]=list2[3].toDouble()/100;
            attitude_cor.attitude_heave_N[0]=list2[4].toDouble()/100;
            attitude_cor.attitude_heading_N[0]=list2[5].toDouble()/100;
            for(int i= 1; i<100;i++) {
                line = read.readLine(); list2=line.split(" ");
                attitude_cor.attitude_msectime_N[i]=list2[1].toUInt();
                attitude_cor.attitude_roll_N[i]=list2[2].toDouble()/100;
                attitude_cor.attitude_pitch_N[i]=list2[3].toDouble()/100;
                attitude_cor.attitude_heave_N[i]=list2[4].toDouble()/100;
                attitude_cor.attitude_heading_N[i]=list2[5].toDouble()/100;
            }
            attitude_correct.push_back(attitude_cor);
        }
    }
    else {
        cout << "Open failed." << endl;
    }
    file.close();
}
void mb_parse_binary::Data_Read_Postion(QString readfile) {
    QFile file(readfile);
    bool openOK = file.open(QIODevice::ReadOnly | QIODevice::Text);
    if(openOK == true) {
        QTextStream read(&file);
        QString line = read.readLine(); QStringList list3;
        while (!read.atEnd()) {
            line = read.readLine(); list3=line.split(" ");
            position_cor.position_date=list3[0].toUInt();
            position_cor.position_msec=list3[1].toUInt();
            position_cor.position_B=list3[2].toDouble()/20000000;
            position_cor.position_L=list3[3].toDouble()/10000000;
            position_correct.push_back(position_cor);
        }
    }
    else {
        cout << "Open failed." << endl;
    }
    file.close();
}
void mb_parse_binary::Data_Read_Seabed(QString readfile) {
    QFile file(readfile);
    bool openOK = file.open(QIODevice::ReadOnly | QIODevice::Text);
    if(openOK == true) {
        QTextStream read(&file);
        QString line = read.readLine(); QStringList list1;
        while (!read.atEnd()) {
            line = read.readLine(); list1=line.split(" ");
            seabed_cor.seabed_pingno=list1[0].toUInt();
            seabed_cor.seabed_beam_N =list1[1].toUInt();
            seabed_cor.seabed_beamno[0] = list1[2].toUInt();
            seabed_cor.seabed_date=list1[3].toUInt();
            seabed_cor.seabed_msec=list1[4].toUInt();
            seabed_cor.count_NS[0]=list1[5].toUInt();
            for(unsigned int i = 0; i < seabed_cor.count_NS[0]; ) {
                seabed_cor.seabed_ampulitudes_NS[0][i] = list1[6+i].toShort();
                i++;
            }
            for( unsigned int j = 1; j < seabed_cor.seabed_beam_N; j++) {
                line ="0";line = read.readLine(); list1=line.split(" ");
                seabed_cor.seabed_beamno[j] = list1[2].toUInt();
                seabed_cor.count_NS[j]=list1[5].toUInt();
                for(unsigned int i = 0; i < seabed_cor.count_NS[j]; ) {
                    seabed_cor.seabed_ampulitudes_NS[j][i] = list1[6+i].toShort();
                    i++;
                }
            }
            seabed_correct.push_back(seabed_cor);
        }
    }
    else {
        cout << "Open failed." << endl;
    }
    file.close();
}

/* Function Name: Correct_Position
 * Inpute Paraments: 需要三个文件的输入：深度、姿态、位置
 * Outpute Paraments:
 * Return:
 * How: 完成坐标时间和测深时间的匹配，导入进最后的结构体
 * Debug:
 * Questions:
 */
void mb_parse_binary::Correct_TS_Position() {
    const int temp_att = attitude_correct.size();
    const int temp_deptht =depth_correct.size();
    const int temp_pos = position_correct.size();
    static int count_time = 0;
    interval_correct.resize(temp_deptht);
//    double S1Z=7.05, S1X=14.47, S1Y=3.23; //声基阵的安装参数，相对于VFS坐标
//    double P1Z=-17.16, P1X=18.03, P1Y=4.66; //GPS的安装参数，相对于VFS坐标
//    double MSZ=0.00, MSX=0.00, MSY=0.00; //姿态仪的安装参数，相对于VFS坐标
//    double S1H=0.36, S1R=-0.11, S1P=-0.03; //声基阵的安装角度参数
//    double MSR=0.02, MSP=0.17, MSG=0.45; //姿态仪的安装角度参数
//    double detla_r=0.13, detla_p=0.48, detla_h=0.34; //两者角度偏差
    double temp_install[18]; //存储安装参数
    temp_install[0] = 7.05; temp_install[1] = 14.47; temp_install[2] = 3.23;
    temp_install[3] = -17.16; temp_install[4] = 18.03; temp_install[5] = 4.66;
    temp_install[6] = 0.00; temp_install[7] = 0.00; temp_install[8] = 0.00;
    temp_install[9] = 0.36; temp_install[10] = -0.11; temp_install[11] = -0.03;
    temp_install[12] = 0.02; temp_install[13] =-0.17 ; temp_install[14] = 0.45;
    temp_install[15] = 0.13; temp_install[16] = 0.20; temp_install[17] = 0.09; //roll pitch heading
    double center = 117; //double temp_a = 6378137.0; double temp_f = 298.257223563; // WGS-84的椭球参数
    for(int count = 0; count<temp_pos; count++) {
        double temp_x = 0.0; double temp_y = 0.0;
        double temp_B = position_correct[count].position_B;
        double temp_L = position_correct[count].position_L;
        double temp_a = 6378137.0; double temp_f = 298.257223563; // WGS-84的椭球参数
        GaussForward(temp_a,temp_f,center,temp_B,temp_L,temp_x,temp_y);
        position_correct[count].position_y = temp_y; position_correct[count].position_x = temp_x; // 计算得到的坐标
    }
    for(int count = 0; count<temp_deptht; count++) {
        unsigned int temp_gpsmsec = position_correct[0].position_msec;
        unsigned int temp_pingmsec = depth_correct[count].depth_msec;
        interval_correct[count].ping_count = depth_correct[count].ping_count;
        if(temp_gpsmsec > temp_pingmsec) {
            interval_correct[count].interval_position_x = 0;
            interval_correct[count].interval_position_y = 0;
            interval_correct[count].interval_position_msec = 0;
            continue;
        }
        for( ; count_time < temp_att; count_time++ ) {
            temp_gpsmsec = position_correct[count_time].position_msec;
            if(temp_gpsmsec < temp_pingmsec)
                continue;
            double temp_x = 0.0; double temp_y = 0.0;
            double temp_x0 = 0.0; double temp_y0 = 0.0;
            double interval_x = 0.0; double interval_y = 0.0;
            unsigned int temp_gpsmsec0 = 0;
            temp_gpsmsec0 = position_correct[count_time-1].position_msec;
            int t10 = temp_gpsmsec - temp_gpsmsec0;
            int ti0 = temp_pingmsec - temp_gpsmsec0;
            temp_x = position_correct[count_time].position_x;
            temp_y = position_correct[count_time].position_y;
            temp_x0 = position_correct[count_time-1].position_x;
            temp_y0 = position_correct[count_time-1].position_y;
            GPS_Interpolation(temp_x0,temp_y0,temp_x,temp_y,t10,ti0,interval_x,interval_y);
            interval_correct[count].interval_position_x = interval_x;
            interval_correct[count].interval_position_y = interval_y;
            interval_correct[count].interval_position_msec = temp_pingmsec;
            break;
        }

    }
    ATT_Interpolation(temp_deptht,temp_att,attitude_correct,interval_correct);
    for(int count = 0; count < temp_deptht; count++) {
        int temp_ping = interval_correct[count].ping_count;
        int temp_ping0 = interval_correct[0].ping_count;
        int check = Position_TS(temp_ping,temp_ping0,interval_correct,temp_install);
        if(check != (temp_ping - temp_ping0))
            cout<<"ERROR in Position_TS";
    }
    for(int ping_count = 0; ping_count < temp_deptht; ping_count++) {
        int temp_ping = interval_correct[ping_count].ping_count;
        int temp_ping0 = interval_correct[0].ping_count;
         int beam_num = depth_correct[0].beam_N;
        interval_correct[ping_count].beam_number = beam_num;
        //Position_Beam(temp_ping,temp_ping0,(beam_num-1),depth_correct,interval_correct);
        int ping_num = temp_ping - temp_ping0;
        if(ping_num > ping_count)
        {
            cout<<"THE DATA is LOST in "<<temp_ping<<endl;
        }
        double ts_x = interval_correct[ping_count].interval_ts_x;
        double ts_y = interval_correct[ping_count].interval_ts_y;
        double ts_z = interval_correct[ping_count].interval_ts_z;
        for(int count = 0; count< beam_num; count++) {
            interval_correct[ping_count].beam_x[count] = double(depth_correct[ping_count].depth_alongtrack_N[count])/100 + ts_x;
            interval_correct[ping_count].beam_y[count] = double(depth_correct[ping_count].depth_acrosstrack_N[count])/100 + ts_y;
            interval_correct[ping_count].beam_z[count] = double(depth_correct[ping_count].depth_120and300_transmit_N[count])/-100 + ts_z;
            interval_correct[ping_count].depthX_N[count] = double(depth_correct[ping_count].depth_alongtrack_N[count])/100;
            interval_correct[ping_count].depthY_N[count] = double(depth_correct[ping_count].depth_acrosstrack_N[count])/100 ;
            interval_correct[ping_count].beam_Reflectivity_N[count] = 256+2*depth_correct[ping_count].Reflectivity_N[count] ;
            interval_correct[ping_count].depthZ_N[count] = double(depth_correct[ping_count].depth_120and300_transmit_N[count])/-100 ;
        }
        if(254 > beam_num)
           { for(int count = beam_num; count< 254; count++) {
                interval_correct[ping_count].beam_x[count] = 0;
                interval_correct[ping_count].beam_y[count] = 0;
                interval_correct[ping_count].beam_z[count] = 0; } }
    }
    cout<<"JJ";
}

/* Name: Gauss_Center
 * Function: 获取中央经线的经度值(3度带)
 * Inpute Paraments: 第一个经度值
 * Outpute Paraments:
 * Return: 中央经线值
 * How: 取整操作
 * Debug:
 * Questions:
 *
 */
int mb_parse_binary::Gauss_Center(double longtitude)
{
        double dx; //需要两个double取整操作
        double center;
        dx=fmod(longtitude,3.0);
        if(dx<=1.5)
        {
            center = longtitude - dx;
        }
        else
        {
            center = longtitude - dx + 3.0;
        }
        return center;
}

/* Name: GaussForward
 * Function: 把经纬度转换为高斯平面坐标（高斯正算）
 * Inpute Paraments: a f 为椭球参数，L0为中央经线维度(3度带)，bd,ld为输入经纬度
 * Outpute Paraments: x,y为输出的坐标值
 * Return: none
 * How:
 * Debug:
 * Questions:
 *
 */
void mb_parse_binary::GaussForward(double a, double f, double center, double bd, double ld, double &xi,  double &yi)
{
    double ep, ee, aa0, aa, bb0, bb, cc0, cc, dd, aa1, bb1, cc1;
    double dd1, c0, c1, c2, c3, ROUD, ROUS, PII, l0, ls, bp;
    double BDPI, CSB, SNB, SQS, X01, X0, t, TT, AIT, N, M0;
    double X, X1, X2, X3, Y, Y1, Y2, Y0;
    ee = 2.0 / f - 1.0 / f / f;
    ep = ee / (1.0 - ee);
    aa0 = 175.0 / 256.0 + ee * (11025.0 / 16384.0 + 43659.0 / 65536.0 * ee);
    aa = 1.0 + ee * (0.75 + ee * (45.0 / 64.0 + ee * aa0));
    bb0 = 525.0 / 512.0 + ee * (2205.0 / 2048.0 + 72765.0 / 65536.0 * ee);
    bb = ee * (0.75 + ee * (15.0 / 16.0 + ee * bb0));
    cc0 = 2205.0 / 4096.0 + ee * 10395.0 / 16384.0;
    cc = ee * ee * (15.0 / 64.0 + ee * (105.0 / 256.0 + ee * cc0));
    dd = ee * ee * ee * (35.0 / 512.0 + ee * (315.0 / 2048.0 + ee * 31185.0 / 13072.0));
    aa1 = aa * a * (1.0 - ee);
    bb1 = -bb * a * (1.0 - ee) / 2.0;
    cc1 = cc * a * (1.0 - ee) / 4.0;
    dd1 = -dd * a * (1.0 - ee) / 6.0;
    c0 = aa1;
    c1 = -(2.0 * bb1 + 4.0 * cc1 + 6.0 * dd1);
    c2 = 8.0 * cc1 + 32.0 * dd1;
    c3 = -32.0 * dd1;
    ROUS = 206264.8063;
    ROUD = 57.29577951;
    PII = 3.1415926535897932384626433832795;

    l0 = center;
    ls = ld - l0;
    ls = ls * 3600;
    bp = PII / 180;
    BDPI = bd * bp;
    CSB = cos(BDPI);
    SNB = sin(BDPI);
    SQS = SNB * SNB;
    X01 = CSB * (c1 * SNB + c2 * SNB * SQS + c3 * SNB * SQS * SQS);
    X0 = c0 * bd / ROUD - X01;
    t = tan(BDPI);
    TT = t * t;
    AIT = ep * CSB * CSB;
    N = a / qSqrt(1.0 - ee * SNB * SNB);
    M0 = ls * CSB / ROUS;
    X1 = N * t * M0 * M0 / 2.0;
    X2 = (5.0 - TT + 9.0 * AIT + 4.0 * AIT * AIT) * X1 * M0 * M0 / 12.0;
    X3 = ((61.0 - 58.0 * t * t + qPow(t, 4)) * X1 * qPow(M0, 4)) / 360.0;
    X = X0 + X1 + X2 + X3;
    X = X / 1000.0;
    Y0 = N * M0;
    Y1 = (1.0 - TT + AIT) * Y0 * M0 * M0 / 6.0;
    Y2 = ((5.0 - 18.0 * TT + TT * TT + 14.0 * AIT - 58.0 * AIT * TT) * Y0 * qPow(M0, 4)) / 120.0;
    Y = Y0 + Y1 + Y2;
    Y = Y / 1000.0;

    xi = X * 1000.0;
    yi = (Y + 500.0) * 1000.0;
}

/* Name: GPS_Interpolation
 * Function: 根据多波束时间插值出来相应时间的GPS位置坐标
 * Inpute Paraments: x0,y0,x1,y1,t10,ti0 分别是 0时刻位置，1时刻位置，求梯度的时间差，待插值时间差
 * Outpute Paraments: interval_x,interval_y 插值得到的结果
 * Return: NONE
 * How: 线性插值 常梯度
 * Debug:
 * Questions:
 *
 */
void mb_parse_binary::GPS_Interpolation(double x0,double y0,double x1, double y1, int t10, int ti0,double &interval_x, double &interval_y)
{
    double DeltaX = (x1 - x0) / t10;
    double DeltaY = (y1 - y0) / t10;
    interval_x = DeltaX * ti0 + x0;
    interval_y = DeltaY * ti0 + y0;
}

/* Name: ATT_Interpolation
 * Function: 姿态的瞬时截取
 * Inpute Paraments: depth_t,attitude_t,attitude_correct,interval_corrtct
 * Outpute Paraments:
 * Return:
 * How: 前后取平均
 * Debug:
 * Questions:
 *
 */
bool mb_parse_binary::ATT_Interpolation(int depth_t,int attitude_t,std::vector<raw_attitude_struct>&attitude_correct,
                                        std::vector<interval_position_attitude_struct>&interval_correct)
{
    for( int count = 0; count < depth_t; count++ ) {
        unsigned int temp_attmsec = attitude_correct[0].attitude_mesc;
        unsigned int temp_pingmsec = depth_correct[count].depth_msec;
        if(temp_attmsec > temp_pingmsec) {
            interval_correct[count].interval_attitude_roll = 0;
            interval_correct[count].interval_attitude_pitch = 0;
            interval_correct[count].interval_attitude_heave = 0;
            interval_correct[count].interval_attitude_heading = 0;
            continue;
        }
        temp_attmsec = attitude_correct[count].attitude_mesc; int count_i = count;
        for(int count_time = 0; count_time < attitude_t; count_time++ ) {
            while (temp_attmsec < temp_pingmsec) {
                temp_attmsec = attitude_correct[count_i].attitude_mesc;
                count_i++;
            }
            unsigned int temp_attmsecn = attitude_correct[count_i-2].attitude_msectime_N[count_time];
            unsigned int temp_attmsecn0 = 0;
            double temp_r0,temp_p0,temp_h0,temp_heading0, temp_r ,temp_p,temp_h,temp_heading;
            if(temp_attmsecn < temp_pingmsec)
                continue;
            if(count_time == 0) {
                temp_attmsecn0 = attitude_correct[count_i-2].attitude_msectime_N[count_time];
                temp_r0 =attitude_correct[count_i-2].attitude_roll_N[count_time];
                temp_p0 =attitude_correct[count_i-2].attitude_pitch_N[count_time];
                temp_h0 =attitude_correct[count_i-2].attitude_heave_N[count_time];
                temp_heading0 =attitude_correct[count_i-2].attitude_heading_N[count_time];
            }
            else {
                temp_attmsecn0 = attitude_correct[count_i-2].attitude_msectime_N[count_time-1];
                temp_r0 =attitude_correct[count_i-2].attitude_roll_N[count_time-1];
                temp_p0 =attitude_correct[count_i-2].attitude_pitch_N[count_time-1];
                temp_h0 =attitude_correct[count_i-2].attitude_heave_N[count_time-1];
                temp_heading0 =attitude_correct[count_i-2].attitude_heading_N[count_time-1];
            }
            temp_r =attitude_correct[count_i-2].attitude_roll_N[count_time];
            temp_p =attitude_correct[count_i-2].attitude_pitch_N[count_time];
            temp_h =attitude_correct[count_i-2].attitude_heave_N[count_time];
            temp_heading =attitude_correct[count_i-2].attitude_heading_N[count_time];
            if(temp_pingmsec-temp_attmsecn0 >temp_attmsecn-temp_pingmsec) {
                interval_correct[count].interval_attitude_roll = temp_r ;
                interval_correct[count].interval_attitude_pitch = temp_p ;
                interval_correct[count].interval_attitude_heave = temp_h ;
                interval_correct[count].interval_attitude_heading = temp_heading;
                interval_correct[count].interval_attitude_mesc = temp_attmsecn;
            }
            else {
                interval_correct[count].interval_attitude_roll = temp_r0 ;
                interval_correct[count].interval_attitude_pitch = temp_p0 ;
                interval_correct[count].interval_attitude_heave = temp_h0 ;
                interval_correct[count].interval_attitude_heading = temp_heading0;
                interval_correct[count].interval_attitude_mesc = temp_attmsecn0;
            }
            break;
        }
    }
    return true;
}

/* Name: Position_TS
 * Function: 处理换能器的瞬时空间位置（大地坐标系）
 * Inpute Paraments: ping为当前ping数，first_ping为第一ping数，install为解析的安装参数
 * Outpute Paraments: ptslls为改正后大地坐标值
 * Return: 当前距初始的ping数
 * How: eigen
 * Debug:
 * Questions:
 *
 */
int mb_parse_binary::Position_TS(int ping,int first_ping,std::vector<interval_position_attitude_struct >&interval_correct,double vec[])
{
    ping = ping - first_ping;
    // 定义转换矩阵(弧度制)
    double detla_r = vec[15]*PI/180; double detla_p = vec[16]*PI/180; double detla_h = vec[17]*PI/180;
    double roll =interval_correct[ping].interval_attitude_roll*PI/180;
    double pitch =interval_correct[ping].interval_attitude_pitch*PI/180;
    double heading =interval_correct[ping].interval_attitude_heading*PI/180;
    roll = asin( cos(detla_h)*sin(roll)+sin(detla_h)*sin(pitch) ); roll = roll + detla_r;
    pitch = asin(  cos(detla_h)*sin(pitch)-sin(detla_h)*sin(roll) ); pitch = pitch + detla_p;
    heading = heading + detla_h;
    MatrixXd Rxy(3,3);
    MatrixXd Rz(3,3);
    MatrixXd Pgps(3,1);
    MatrixXd Pts0(3,1);
    MatrixXd Ptslls(3,1);
    Rxy << cos(pitch), sin(roll)*sin(pitch), cos(roll)*sin(pitch),
            0, cos(roll), -sin(roll),
            -sin(pitch), sin(roll)*cos(pitch),cos(roll)*cos(pitch);
    Rz <<cos(heading), -sin(heading), 0,
            sin(heading), cos(heading), 0,
            0, 0, 1;
    Pgps <<  interval_correct[ping].interval_position_x -  interval_correct[0].interval_position_x,
            interval_correct[ping].interval_position_y -  interval_correct[0].interval_position_y,
            -interval_correct[ping].interval_attitude_heave;
    Pts0 <<vec[1],
            vec[2],
            vec[0];
    Ptslls << 0,0,0;
    Ptslls = Rz*Rxy*Pts0;
    Ptslls = Ptslls + Pgps;//前者为姿态改正值,后者为位置改正值
    interval_correct[ping].interval_ts_x = Ptslls(0,0);
    interval_correct[ping].interval_ts_y =Ptslls(1,0);
    interval_correct[ping].interval_ts_z =Ptslls(2,0);
    return ping;
}

/* Name: Position_Beam
 * Function: 解算一PING中每一个beam的大地坐标,beam不足254部分赋为0
 * Inpute Paraments: beam_num为从0开始的总beam数
 * Outpute Paraments:
 * Return:
 * How:
 * Debug:
 * Questions:
 *
 */
void mb_parse_binary::Position_Beam(int ping,int first_ping,int beam_num, std::vector<raw_depth_struct>&depth_correct,
                                    std::vector<interval_position_attitude_struct>&interval_correct)
{
    int ping_num = ping - first_ping;
    for(int count = 0; count< beam_num; count++) {
        interval_correct[ping_num].beam_x[count] = depth_correct[ping_num].depth_alongtrack_N[count]
                + interval_correct[ping_num].interval_ts_x;
        interval_correct[ping_num].beam_y[count] = depth_correct[ping_num].depth_acrosstrack_N[count]
                + interval_correct[ping_num].interval_ts_y;
        interval_correct[ping_num].beam_z[count] = depth_correct[ping_num].depth_transmit_N[count]
                + interval_correct[ping_num].interval_ts_z;
    }
    if(254 > beam_num)
       { for(int count = beam_num; count< 254; count++) {
            interval_correct[ping_num].beam_x[count] = 0;
            interval_correct[ping_num].beam_y[count] = 0;
            interval_correct[ping_num].beam_z[count] = 0; } }
}

/* Name: Print_Interval
 * Function: 打印完整的数据结构体
 * Inpute Paraments: 文件流
 * Outpute Paraments:
 * Return:
 * How:
 * Debug:
 * Questions:
 */
void mb_parse_binary::Print_Interval(QString writefile)
{
    QFile file(writefile);
    bool openOK = file.open(QIODevice::WriteOnly);
    if(openOK == true) {
        QTextStream write(&file);
        for(unsigned int count = 0; count < interval_correct.size(); count++) {
            write<<interval_correct[count].ping_count<<" "
            <<interval_correct[count].interval_position_msec<<" "
            <<interval_correct[count].interval_position_x<<" "
            <<interval_correct[count].interval_position_y<<" "
            <<interval_correct[count].interval_attitude_mesc<<" "
            <<interval_correct[count].interval_attitude_roll<<" "
            <<interval_correct[count].interval_attitude_pitch<<" "
            <<interval_correct[count].interval_attitude_heading<<" "
            <<interval_correct[count].interval_attitude_heave<<" "
            <<interval_correct[count].interval_ts_x<<" "
            <<interval_correct[count].interval_ts_y<<" "
            <<interval_correct[count].interval_ts_z<<" "
            <<interval_correct[count].beam_number<<endl;
            for(unsigned int count_beam = 0; count_beam <interval_correct[count].beam_number;count_beam++ ) {
                write<<interval_correct[count].beam_x[count_beam]<<" "
                 <<interval_correct[count].beam_y[count_beam]<<" "
                 <<interval_correct[count].beam_z[count_beam]<<endl;
            }
            if(interval_correct[count].beam_x[interval_correct[count].beam_number] != 0)
                cout<<"THE DATA BEAM NUMBER IS WRONG";
        }
    }
    file.close();
}
void mb_parse_binary::Print_BS(QString writefile) {
    QFile file(writefile);
    bool openOK = file.open(QIODevice::WriteOnly);
    if(openOK == true) {
        QTextStream write(&file);
        for(unsigned int count = 0; count < interval_correct.size(); count++) {
            for(unsigned int count_beam = 0; count_beam <interval_correct[count].beam_number;count_beam++ ) {
                write<<interval_correct[count].beam_x[count_beam]<<" "
                <<interval_correct[count].beam_y[count_beam]<<" "
                <<interval_correct[count].beam_z[count_beam]<<" "
                <<interval_correct[count].beam_Reflectivity_N[count_beam]<<endl;

            }
            if(interval_correct[count].beam_x[interval_correct[count].beam_number] != 0)
                cout<<"THE DATA BEAM NUMBER IS WRONG";
        }
    }
    file.close();
}
/* Name: Depth_GaussFilter
 * Function: 完成水深数据的GaussFilter算法滤波并输出
 * Inpute Paraments:
 * Outpute Paraments:
 * Return:
 * How:
 * Debug:
 * Questions:
 *
 */
int mb_parse_binary::Depth_GaussFilter()
{

    return 1;
}

/* Function Name: BS_Read
 * Inpute Paraments: 待底质分类数据-按师姐的格式读取
 * Outpute Paraments: NONE
 * Return:
 * How:
 * Debug:
 * Questions:
 */
void mb_parse_binary::BS_Read(QString readfile) {
    QFile file(readfile);
    bool openOK = file.open(QIODevice::ReadOnly | QIODevice::Text);
    if(openOK == true) {
        QTextStream read(&file);
        QString line = read.readLine(); QStringList list1;
        while (!read.atEnd()) {
            line = read.readLine(); list1=line.split(" ");
            bs_cor.Number=list1[0].toUInt();
            bs_cor.X=list1[1].toDouble();
            bs_cor.Y=list1[2].toDouble();
            bs_cor.Z=list1[3].toDouble();
            bs_cor.Reflectivity=list1[4].toShort();
            bs_cor.ASM=list1[5].toDouble();
            bs_cor.ENT=list1[6].toDouble();
            bs_cor.CORR=list1[7].toDouble();
            bs_cor.CON=list1[8].toDouble();
            bs_correct.push_back(bs_cor);
        }
    }
    else {
        cout << "Open failed." << endl;
    }
    file.close();
}

/* Name: 需要注意其中指针存在问题
 * Function:
 * Inpute Paraments:
 * Outpute Paraments:
 * Return:
 * How:
 * Debug:
 * Questions:
 *
 */

//void mb_parse_binary::fuc_swap_check(char buffer)
//{
////    int *value = NULL; int *value2 = NULL;
////    memcpy(value, buffer, sizeof(int));
////    memcpy(value2, buffer, sizeof(int));
////    *value2 = mb_swap_int(*value2);
////    if(*value<*value2) {
////        value = (int *) ptr;
////        memcpy(value, buffer, sizeof(int));
////    }
////    else {
////        value2 = (int *)ptr;
////        memcpy(value2, buffer, sizeof(int));
////        *value2 = mb_swap_int(*value2);
////       #define BYTESWAPPED
////    }

//    int value = 0;
//    int value_swap = 0;
//    value =(int)buffer;
//    value_swap = (int)buffer;
//        if(value<value_swap)
//           #define BYTESWAPPED
//}

/*-----------------------具体读取函数--------------------------*/
/* Name: mb_get_binary_short
 * Function: 真正的赋值函数，把值由temp_char赋给结构体
 * Inpute Paraments:
 *           @1 void *buffer 一般为&temp_char[index]形式(存数据的值)
 *           @2 void *ptr 一般为&depth.depth_xx形式(要赋的值)
 * Outpute Paraments: None
 * Return:  0
 * How:   创建指针。绑定地址，memcpy函数完成赋值
 * Debug:
 * Questions: 记得查看不同类型时类型转换的问题
 *
 */
int mb_parse_binary::mb_get_binary_short(void *buffer, void *ptr)
{
    short *value;

    value = (short *) ptr;
    memcpy(value, buffer, sizeof(short));
#ifdef BYTESWAPPED
        *value = mb_swap_short(*((short *)value));
#endif
    return(0);
}
int mb_parse_binary::mb_get_binary_int(void *buffer, void *ptr)
{
    int *value;

    value = (int *) ptr;
    memcpy(value, buffer, sizeof(int));
#ifdef BYTESWAPPED
        *value = mb_swap_int(*value);
#endif
    return(0);
}
int mb_parse_binary::mb_get_binary_float(void *buffer, void *ptr)
{
    float *value;

    value = (float *) ptr;
    memcpy(value, buffer, sizeof(float));
#ifdef BYTESWAPPED
        mb_swap_float(value);
#endif
    return(0);
}
int mb_parse_binary::mb_get_binary_double(void *buffer, void *ptr)
{
    double *value;

    value = (double *) ptr;
    memcpy(value, buffer, sizeof(double));
#ifdef BYTESWAPPED
        mb_swap_double(value);
#endif
    return(0);
}
int mb_parse_binary::mb_get_binary_long(void *buffer, void *ptr)
{
    mb_s_long *value;

    value = (mb_s_long *) ptr;
    memcpy(value, buffer, sizeof(mb_s_long));
#ifdef BYTESWAPPED
        mb_swap_long(value);
#endif
    return(0);
}

/*---------------------底层交换函数--------------------------*/
/* Name: mb_swap_float()
 * Function: 处理大小端问题,交换
 * Inpute Paraments: 对应类型的数据指针
 * Outpute Paraments: None
 * Return:
 *              返回值表示交换成功
 * How:
 *              与头文件中#define的两个同属底层
 */
int mb_parse_binary::mb_swap_float(float *a)
{
    unsigned int	*t;
    t = (unsigned int *) a;
    *t = mb_swap_int(*t);

    return(MB_SUCCESS);
}
int mb_parse_binary::mb_swap_double(double *a)
{
    mb_u_char bc[8];
    mb_u_char *ac;

    ac = (mb_u_char *) a;
    bc[0] = ac[7];
    bc[1] = ac[6];
    bc[2] = ac[5];
    bc[3] = ac[4];
    bc[4] = ac[3];
    bc[5] = ac[2];
    bc[6] = ac[1];
    bc[7] = ac[0];
    ac[0] = bc[0];
    ac[1] = bc[1];
    ac[2] = bc[2];
    ac[3] = bc[3];
    ac[4] = bc[4];
    ac[5] = bc[5];
    ac[6] = bc[6];
    ac[7] = bc[7];

    return(MB_SUCCESS);
}
int mb_parse_binary::mb_swap_long(mb_s_long *a)
{
    mb_u_char bc[8];
    mb_u_char *ac;

    ac = (mb_u_char *) a;
    bc[0] = ac[7];
    bc[1] = ac[6];
    bc[2] = ac[5];
    bc[3] = ac[4];
    bc[4] = ac[3];
    bc[5] = ac[2];
    bc[6] = ac[1];
    bc[7] = ac[0];
    ac[0] = bc[0];
    ac[1] = bc[1];
    ac[2] = bc[2];
    ac[3] = bc[3];
    ac[4] = bc[4];
    ac[5] = bc[5];
    ac[6] = bc[6];
    ac[7] = bc[7];

    return(MB_SUCCESS);
}

//------------------------开始底质分类代码--------------------------------------
/* Function Name: trainSVM
 * Inpute Paraments: const string& modelFileName 用来保存训练完成的模型
 * Outpute Paraments:
 * Return:
 * How:
 * Debug:
 * Questions:
 */
void mb_parse_binary::trainSVM(QString readfile, const std::string& modelFileName) {
    setParam();
    const std::string readfiles = readfile.toStdString();
    //const std::string readfile2= "d://A120//Train.txt";
    char input_file_name[1024]; strcpy(input_file_name,readfiles.c_str());
    read_problem(input_file_name);
    const char *error_msg;
    error_msg = svm_check_parameter(&prob,&param);
    cout << "error_msg" << error_msg<< endl;
    cout << "start training" << endl;
    svm_model *svmModel = svm_train(&prob, &param);
    cout << "save model" << endl;
    svm_save_model(modelFileName.c_str(), svmModel);
    cout << "done!" << endl;
    free(prob.y);
    free(prob.x);
    free(x_space);
    free(line);
}

/* Function Name: predictSVM
 * Inpute Paraments:
 * Outpute Paraments:
 * Return:
 * How: 预测单个模型
 * Debug:
 * Questions:
 */
void mb_parse_binary::predictSVM(QString readfile, const std::string& modelFileName) {
    cout<<"predictSVM start!"<<endl;
    svm_model *svmModel = svm_load_model(modelFileName.c_str());
    QFile file(readfile);
    bool openOK = file.open(QIODevice::ReadOnly | QIODevice::Text);
    if(openOK == true) {
        QTextStream read(&file);
        QString line = read.readLine(); QStringList list1;
        list1=line.split(" "); int FEATUREDIM = list1[0].toInt(); //FEATUREDIM表示特征的维度
        //svm_node* features = new svm_node[FEATUREDIM+1];
        int j = 1; int type = 0; int correct_j = 0;
        while (!read.atEnd()) {
            svm_node* input = new svm_node[FEATUREDIM + 1];
            line = read.readLine(); list1=line.split(" ");  type =list1[0].toInt();
            for( int i = 0; i < FEATUREDIM; i++) {
                input[i].index = list1[2*i+1].toInt(); //特征标号，从1开始
                input[i].value = list1[2*i+2].toDouble(); //特征值
            }
            input[FEATUREDIM].index = -1;
            double predictValue = svm_predict(svmModel, input);
            //cout<<"type predictValue"<<j<<"is"<<type<<predictValue<<endl;
            if(type == predictValue)
                correct_j++;
            j++;
        }
        cout<<"All Number IS"<<j<<"RightPredict IS"<<correct_j<<endl;
    }
    file.close();
}

/* Function Name: readTestData
 * Inpute Paraments: readfile 文件句柄 FEATUREDIM 特征维度
 * Outpute Paraments:
 * Return:
 * How: 按格式读取预测函数集
 * Debug:
 * Questions:
 */
void mb_parse_binary::readTestData(QString readfile )
{
    QFile file(readfile);
    bool openOK = file.open(QIODevice::ReadOnly | QIODevice::Text);
    if(openOK == true) {
        QTextStream read(&file);
        QString line = read.readLine(); QStringList list1;
        list1=line.split(" "); int FEATUREDIM = list1[0].toUInt(); //FEATUREDIM表示特征的维度
        svm_node* features = new svm_node[FEATUREDIM+1];
        int type = 0; SampleNum = 0;
        while (!read.atEnd()) {
            line = read.readLine(); list1=line.split(" "); type =list1[0].toInt();
            for( int i = 0; i < FEATUREDIM; i++) {
                features[i].index = list1[2*i+1].toInt(); //特征标号，从1开始
                features[i].value = list1[2*i+2].toDouble(); //特征值
                // cout<<features[i].index<<" "<<features[i].value<<endl;
            }
            features[FEATUREDIM].index = -1;
            dataList.push_back(features);
            typeList.push_back(type);
            SampleNum++;
        }
    }
    file.close();
}

/* Function Name: setParam exit_input_error read_problem
 * Inpute Paraments:
 * Outpute Paraments:
 * Return:
 * How: 设置模型参数-接口 设置阅读函数
 * Debug:
 * Questions:
 */
void mb_parse_binary::setParam()
{
    param.svm_type = C_SVC;
    param.kernel_type = RBF;
    param.degree = 3;
    param.gamma = 0;	// 1/num_features
    param.coef0 = 0;
    param.nu = 0.5;
    param.cache_size = 100;
    param.C = 1;
    param.eps = 1e-3;
    param.p = 0.1;
    param.shrinking = 1;
    param.probability = 0;
    param.nr_weight = 0;
    param.weight_label = NULL;
    param.weight = NULL;
}
void mb_parse_binary::exit_input_error(int line_num)
{
    fprintf(stderr,"Wrong input format at line %d\n", line_num);
    exit(1);
}
static char* readline(FILE *input)
{
    int len;

    if(fgets(line,max_line_len,input) == NULL)
        return NULL;

    while(strrchr(line,'\n') == NULL)
    {
        max_line_len *= 2;
        line = (char *) realloc(line,max_line_len);
        len = (int) strlen(line);
        if(fgets(line+len,max_line_len-len,input) == NULL)
            break;
    }
    return line;
}
void mb_parse_binary::read_problem(const char *filename)
{
    int max_index, inst_max_index, i;
    size_t elements, j;
    FILE *fp = fopen(filename,"r");
    char *endptr;
    char *idx, *val, *label;

    if(fp == NULL)
    {
        fprintf(stderr,"can't open input file %s\n",filename);
        exit(1);
    }

    prob.l = 0;
    elements = 0;

    max_line_len = 1024;
    line = Malloc(char,max_line_len);
    while(readline(fp)!=NULL)
    {
        char *p = strtok(line," \t"); // label

        // features
        while(1)
        {
            p = strtok(NULL," \t");
            if(p == NULL || *p == '\n') // check '\n' as ' ' may be after the last feature
                break;
            ++elements;
        }
        ++elements;
        ++prob.l;
    }
    rewind(fp);

    prob.y = Malloc(double,prob.l);
    prob.x = Malloc(struct svm_node *,prob.l);
    x_space = Malloc(struct svm_node,elements);

    max_index = 0;
    j=0;
    for(i=0;i<prob.l;i++)
    {
        inst_max_index = -1; // strtol gives 0 if wrong format, and precomputed kernel has <index> start from 0
        readline(fp);
        prob.x[i] = &x_space[j];
        label = strtok(line," \t\n");
        if(label == NULL) // empty line
            exit_input_error(i+1);

        prob.y[i] = strtod(label,&endptr);
        if(endptr == label || *endptr != '\0')
            exit_input_error(i+1);

        while(1)
        {
            idx = strtok(NULL,":");
            val = strtok(NULL," \t");

            if(val == NULL)
                break;

            errno = 0;
            x_space[j].index = (int) strtol(idx,&endptr,10);
            if(endptr == idx || errno != 0 || *endptr != '\0' || x_space[j].index <= inst_max_index)
                exit_input_error(i+1);
            else
                inst_max_index = x_space[j].index;

            errno = 0;
            x_space[j].value = strtod(val,&endptr);
            if(endptr == val || errno != 0 || (*endptr != '\0' && !isspace(*endptr)))
                exit_input_error(i+1);

            ++j;
        }

        if(inst_max_index > max_index)
            max_index = inst_max_index;
        x_space[j++].index = -1;
    }

    if(param.gamma == 0 && max_index > 0)
        param.gamma = 1.0/max_index;

    if(param.kernel_type == PRECOMPUTED)
        for(i=0;i<prob.l;i++)
        {
            if (prob.x[i][0].index != 0)
            {
                fprintf(stderr,"Wrong input format: first column must be 0:sample_serial_number\n");
                exit(1);
            }
            if ((int)prob.x[i][0].value <= 0 || (int)prob.x[i][0].value > max_index)
            {
                fprintf(stderr,"Wrong input format: sample_serial_number out of range\n");
                exit(1);
            }
        }

    fclose(fp);
}


//------------------------kmeans底质分类代码--------------------------------------
void mb_parse_binary::KMeans_First(int dimNum, int clusterNum, int iteriation)
{
    m_dimNum = dimNum;
    m_clusterNum = clusterNum;

    m_means = new double*[m_clusterNum];
    for(int i = 0; i < m_clusterNum; i++)
    {
        m_means[i] = new double[m_dimNum];
        memset(m_means[i], 0, sizeof(double) * m_dimNum);
    }

    m_initMode = InitRandom;
    m_maxIterNum = iteriation;
    m_endError = 0.001;
}
void mb_parse_binary::KMeans_After()
{
    for(int i = 0; i < m_clusterNum; i++)
    {
        delete[] m_means[i];
    }
    delete[] m_means;
}

void mb_parse_binary::Cluster(const char* sampleFileName, const char* labelFileName)
{
    // Check the sample file
    ifstream sampleFile(sampleFileName, ios_base::binary);
    assert(sampleFile);

    int size = 0;
    int dim = 0;
    sampleFile.read((char*)&size, sizeof(int));
    sampleFile.read((char*)&dim, sizeof(int));
    assert(size >= m_clusterNum);
    assert(dim == m_dimNum);

    // Initialize model
    Init(sampleFile);

    // Recursion
    double* x = new double[m_dimNum];	// Sample data
    int label = -1;		// Class index
    double iterNum = 0;
    double lastCost = 0;
    double currCost = 0;
    int unchanged = 0;
    bool loop = true;
    int* counts = new int[m_clusterNum];
    double** next_means = new double*[m_clusterNum];	// New model for reestimation
    for(int i = 0; i < m_clusterNum; i++)
    {
        next_means[i] = new double[m_dimNum];
    }

    while(loop)
    {
        //clean buffer for classification
        memset(counts, 0, sizeof(int) * m_clusterNum);
        for(int i = 0; i < m_clusterNum; i++)
        {
            memset(next_means[i], 0, sizeof(double) * m_dimNum);
        }

        lastCost = currCost;
        currCost = 0;

        sampleFile.clear();
        sampleFile.seekg(sizeof(int) * 2, ios_base::beg);

        // Classification
        for(int i = 0; i < size; i++)
        {
            sampleFile.read((char*)x, sizeof(double) * m_dimNum);
            currCost += GetLabel(x, &label);

            counts[label]++;
            for(int d = 0; d < m_dimNum; d++)
            {
                next_means[label][d] += x[d];
            }
        }
        currCost /= size;

        // Reestimation
        for(int i = 0; i < m_clusterNum; i++)
        {
            if(counts[i] > 0)
            {
                for(int d = 0; d < m_dimNum; d++)
                {
                    next_means[i][d] /= counts[i];
                }
                memcpy(m_means[i], next_means[i], sizeof(double) * m_dimNum);
            }
        }

        // Terminal conditions
        iterNum++;
        if(fabs(lastCost - currCost) < m_endError * lastCost)
        {
            unchanged++;
        }
        if(iterNum >= m_maxIterNum || unchanged >= 3)
        {
            loop = false;
        }
        //DEBUG
        //cout << "Iter: " << iterNum << ", Average Cost: " << currCost << endl;
    }

    // Output the label file
    ofstream labelFile(labelFileName, ios_base::binary);
    assert(labelFile);

    labelFile.write((char*)&size, sizeof(int));
    sampleFile.clear();
    sampleFile.seekg(sizeof(int) * 2, ios_base::beg);

    for(int i = 0; i < size; i++)
    {
        sampleFile.read((char*)x, sizeof(double) * m_dimNum);
        GetLabel(x, &label);
        labelFile.write((char*)&label, sizeof(int));
    }

    sampleFile.close();
    labelFile.close();

    delete[] counts;
    delete[] x;
    for(int i = 0; i < m_clusterNum; i++)
    {
        delete[] next_means[i];
    }
    delete[] next_means;
}

//N 为特征向量数
void mb_parse_binary::Cluster( double *data, int N, int *Label)
{
    int size = 0;
        size = N;

        assert(size >= m_clusterNum);

        // Initialize model
        Init(data,N);

        // Recursion
        double* x = new double[m_dimNum];	// Sample data
        int label = -1;		// Class index
        double iterNum = 0;
        double lastCost = 0;
        double currCost = 0;
        int unchanged = 0;
        bool loop = true;
        int* counts = new int[m_clusterNum];
        double** next_means = new double*[m_clusterNum];	// New model for reestimation
        for(int i = 0; i < m_clusterNum; i++)
        {
            next_means[i] = new double[m_dimNum];
        }

        while(loop)
        {
            //clean buffer for classification
            memset(counts, 0, sizeof(int) * m_clusterNum);
            for(int i = 0; i < m_clusterNum; i++)
            {
                memset(next_means[i], 0, sizeof(double) * m_dimNum);
            }

            lastCost = currCost;
            currCost = 0;

            // Classification
            for(int i = 0; i < size; i++)
            {
                for(int j = 0; j < m_dimNum; j++)
                    x[j] = data[i*m_dimNum+j];

                currCost += GetLabel(x, &label);

                counts[label]++;
                for(int d = 0; d < m_dimNum; d++)
                {
                    next_means[label][d] += x[d];
                }
            }
            currCost /= size;

            // Reestimation
            for(int i = 0; i < m_clusterNum; i++)
            {
                if(counts[i] > 0)
                {
                    for(int d = 0; d < m_dimNum; d++)
                    {
                        next_means[i][d] /= counts[i];
                    }
                    memcpy(m_means[i], next_means[i], sizeof(double) * m_dimNum);
                }
            }

            // Terminal conditions
            iterNum++;
            if(fabs(lastCost - currCost) < m_endError * lastCost)
            {
                unchanged++;
            }
            if(iterNum >= m_maxIterNum || unchanged >= 3)
            {
                loop = false;
            }

            //DEBUG
            //cout << "Iter: " << iterNum << ", Average Cost: " << currCost << endl;
        }

        // Output the label file
        for(int i = 0; i < size; i++)
        {
            for(int j = 0; j < m_dimNum; j++)
                x[j] = data[i*m_dimNum+j];
            GetLabel(x,&label);
            Label[i] = label;
        }
        delete[] counts;
        delete[] x;
        for(int i = 0; i < m_clusterNum; i++)
        {
            delete[] next_means[i];
        }
        delete[] next_means;
}

void mb_parse_binary::Init(double *data, int N)
{
    int size = N;

    if(m_initMode ==  InitRandom)
    {
        int inteval = size / m_clusterNum;
        double* sample = new double[m_dimNum];

        // Seed the random-number generator with current time
        srand((unsigned)time(NULL));

        for(int i = 0; i < m_clusterNum; i++)
        {
            int select = inteval * i + (inteval - 1) * rand() / RAND_MAX;
            for(int j = 0; j < m_dimNum; j++)
                sample[j] = data[select*m_dimNum+j];
            memcpy(m_means[i], sample, sizeof(double) * m_dimNum);
        }

        delete[] sample;
    }
    else if(m_initMode == InitUniform)
    {
        double* sample = new double[m_dimNum];

        for(int i = 0; i < m_clusterNum; i++)
        {
            int select = i * size / m_clusterNum;
            for(int j = 0; j < m_dimNum; j++)
                sample[j] = data[select*m_dimNum+j];
            memcpy(m_means[i], sample, sizeof(double) * m_dimNum);
        }

        delete[] sample;
    }
    else if(m_initMode == InitManual)
    {
        // Do nothing
    }
}

void mb_parse_binary::Init(ifstream& sampleFile)
{
    int size = 0;
    sampleFile.seekg(0, ios_base::beg);
    sampleFile.read((char*)&size, sizeof(int));

    if(m_initMode ==  InitRandom)
    {
        int inteval = size / m_clusterNum;
        double* sample = new double[m_dimNum];

        // Seed the random-number generator with current time
        srand((unsigned)time(NULL));

        for(int i = 0; i < m_clusterNum; i++)
        {
            int select = inteval * i + (inteval - 1) * rand() / RAND_MAX;
            int offset = sizeof(int) * 2 + select * sizeof(double) * m_dimNum;

            sampleFile.seekg(offset, ios_base::beg);
            sampleFile.read((char*)sample, sizeof(double) * m_dimNum);
            memcpy(m_means[i], sample, sizeof(double) * m_dimNum);
        }

        delete[] sample;
    }
    else if(m_initMode == InitUniform)
    {
        double* sample = new double[m_dimNum];

        for (int i = 0; i < m_clusterNum; i++)
        {
            int select = i * size / m_clusterNum;
            int offset = sizeof(int) * 2 + select * sizeof(double) * m_dimNum;

            sampleFile.seekg(offset, ios_base::beg);
            sampleFile.read((char*)sample, sizeof(double) * m_dimNum);
            memcpy(m_means[i], sample, sizeof(double) * m_dimNum);
        }

        delete[] sample;
    }
    else if(m_initMode == InitManual)
    {
        // Do nothing
    }
}

double mb_parse_binary::GetLabel(const double* sample, int* label)
{
    double dist = -1;
    for(int i = 0; i < m_clusterNum; i++)
    {
        double temp = CalcDistance(sample, m_means[i], m_dimNum);
        if(temp < dist || dist == -1)
        {
            dist = temp;
            *label = i;
        }
    }
    return dist;
}

double mb_parse_binary::CalcDistance(const double* x, const double* u, int dimNum)
{
    double temp = 0;
    for(int d = 0; d < dimNum; d++)
    {
        temp += (x[d] - u[d]) * (x[d] - u[d]);
    }
    return sqrt(temp);
}

ostream& operator<<(ostream& out, mb_parse_binary& kmeans)
{
    out << "<KMeans>" << endl;
    out << "<DimNum> " << kmeans.m_dimNum << " </DimNum>" << endl;
    out << "<ClusterNum> " << kmeans.m_clusterNum << " </CluterNum>" << endl;

    out << "<Mean>" << endl;
    for(int i = 0; i < kmeans.m_clusterNum; i++)
    {
        for(int d = 0; d < kmeans.m_dimNum; d++)
        {
            out << kmeans.m_means[i][d] << " ";
        }
        out << endl;
    }
    out << "</Mean>" << endl;

    out << "</KMeans>" << endl;
    return out;
}

void mb_parse_binary::Data_Read_kmeans(QString readfile, double *data) {
    QFile file(readfile);
    bool openOK = file.open(QIODevice::ReadOnly | QIODevice::Text);
    if(openOK == true) {
        QTextStream read(&file);
        QString line = read.readLine(); QStringList list1;
        while (!read.atEnd()) {
            line = read.readLine(); list1=line.split(" ");
            bs_cor.bs_x=list1[0].toDouble();
            bs_cor.bs_y=list1[1].toDouble();
            bs_cor.bs_z=list1[2].toDouble();
            bs_cor.BS=list1[3].toDouble();
            bs_correct.push_back(bs_cor);
        }
    }
    file.close();
    for(unsigned int i = 0; i < bs_correct.size(); i++) {
        data[4*i] = bs_correct[i].bs_x;
        data[4*i+1] = bs_correct[i].bs_y;
        data[4*i+2] = bs_correct[i].bs_z;
        data[4*i+3] = bs_correct[i].BS;
    }
}

void mb_parse_binary::Data_Print_Kmeans(QString writefile, int *labels) {
    for (unsigned int i = 0; i < bs_correct.size(); i++) {
        bs_correct[i].iso_classify = labels[i];
    }
    QFile file(writefile);
    bool openOK = file.open(QIODevice::WriteOnly);
    if(openOK == true) {
        QTextStream write(&file);
        for(unsigned int j = 0; j < bs_correct.size(); j++) {
             write<<bs_correct[j].bs_x<<" ";
             write<<bs_correct[j].bs_y<<" ";
             write<<bs_correct[j].bs_z<<" ";
             write<<bs_correct[j].BS<<" ";
             write<<bs_correct[j].iso_classify<<endl;
         }
    }
    file.close();
}

void mb_parse_binary::Kmeans_Cluster(QString readfile, int dim, int size, int cluster_num,int iteriation, QString writefile) {
    double data[size*dim];  int* labels = new int[size];
    Data_Read_kmeans(readfile, data); //Number of samples
    KMeans_First(dim, cluster_num, iteriation);
    m_initMode=mb_parse_binary::InitUniform;
    Cluster(data,size,labels);
//    Data_ClassifyResults_Input(labels);
    Data_Print_Kmeans(writefile,labels);
    KMeans_After();
    delete []labels;
}






